#include "replanning.h"

Replanning::Replanning() {}

Replanning::~Replanning() {}

void Replanning::calcGridmapData( const nav_msgs::msg::OccupancyGrid::Ptr image_map_ ) {
    RCLCPP_INFO( rclcpp::get_logger( "" ), "Calculate map_data" );
    bHasMap   = true;
    image_map = image_map_;
    Eigen::Vector2i map_size_xy( image_map->info.width, image_map->info.height );
    map_data.resize( map_size_xy.x(), vector< bool >( map_size_xy.y(), false ) );
    for ( int h = 0; h < map_size_xy.y(); ++h ) {
        for ( int w = 0; w < map_size_xy.x(); ++w ) {
            int index = w + h * map_size_xy.x();
            if ( image_map->data[ index ] ) {
                map_data[ w ][ h ] = true;
            }
        }
    }
    static_map_data = map_data;
}

void Replanning::updateGridmap( const vector< Eigen::Vector3d > &normal_obstacles_,
                                const vector< Eigen::Vector3d > &vehicle_obstacles_ ) {
    normal_obstacles  = normal_obstacles_;
    vehicle_obstacles = vehicle_obstacles_;
    if ( !bHasMap ) {
        RCLCPP_INFO( rclcpp::get_logger( "" ), "Map is not be find!!!" );
    }
    map_data = static_map_data;
    // 障碍物坐标是相对世界坐标系的，而地图坐标系相对于世界坐标系进行了平移，量值为map.yaml文件中的origin,计算栅格索引需要障碍物相对于地图坐标系的坐标
    RCLCPP_INFO( rclcpp::get_logger( "" ), "Update map data" );
    Eigen::Vector2d image_map_origin_xy( image_map->info.origin.position.x, image_map->info.origin.position.y );
    for ( const auto &nor_obs : normal_obstacles ) {
    }
    for ( const auto &veh_obs : vehicle_obstacles ) {
        getVehGridMap( veh_obs );
    }
}

void Replanning::getVehGridMap( const Eigen::Vector3d &veh_obs_pos ) {
    /**
     *         b
     *  a  ---------------- c
     *    |    |          |    Front
     *    |    |          |
     *  d  ----------------
     */
    Eigen::Vector2d veh_obs_pos_for_pixel =
        veh_obs_pos.head( 2 ) - Eigen::Vector2d( image_map->info.origin.position.x, image_map->info.origin.position.y );
    Eigen::Matrix2d rotation;
    rotation << cos( veh_obs_pos.z() ), -sin( veh_obs_pos.z() ), sin( veh_obs_pos.z() ), cos( veh_obs_pos.z() );
    Eigen::Vector2d lower_left_corner_point( -Constants::veh_Lr, -Constants::veh_W / 2 );  // d点相对于车身坐标系坐标
    int             grid_size_num_x = ( Constants::veh_Lf + Constants::veh_Lr ) / image_map->info.resolution;
    int             grid_size_num_y = Constants::veh_W / image_map->info.resolution;
    for ( int i = 0; i < grid_size_num_x; i++ ) {
        for ( int j = 0; j < grid_size_num_y; j++ ) {
            Eigen::Vector2d curr_xy_for_veh =
                lower_left_corner_point
                + Eigen::Vector2d( static_cast< double >( i ) / grid_size_num_x
                                       * ( Constants::veh_Lf + Constants::veh_Lr ),
                                   static_cast< double >( j ) / grid_size_num_y * Constants::veh_W );
            Eigen::Vector2d curr_xy_for_pixel = veh_obs_pos_for_pixel + rotation * curr_xy_for_veh;

            int grid_index_x                         = int( curr_xy_for_pixel[ 0 ] / image_map->info.resolution );
            int grid_index_y                         = int( curr_xy_for_pixel[ 1 ] / image_map->info.resolution );
            map_data[ grid_index_x ][ grid_index_y ] = true;
        }
    }
}

void Replanning::setRefTraj( const vector< traj > &last_ref_traj_ ) {
    last_ref_traj = last_ref_traj_;
    RCLCPP_INFO( rclcpp::get_logger( "" ), "Output last_ref_traj" );
    // for ( const auto &t : last_ref_traj ) {
    //     RCLCPP_INFO( rclcpp::get_logger( "" ), "last_ref_traj: [%.3f,%.3f,%.3f,%.2f]", t.x, t.y, t.phi, t.vel );
    // }
}

bool Replanning::isPointInRectangle( const Eigen::Vector2d &p, const Eigen::Vector3d &veh_obs ) {
    Eigen::Matrix2d Rt;
    Rt << cos( veh_obs.z() ), sin( veh_obs.z() ), -sin( veh_obs.z() ), cos( veh_obs.z() );
    Eigen::Vector2d localXY = Rt * ( p - veh_obs.head( 2 ) );  // 将点 p 转换到车身坐标系

    bool inside = localXY.x() >= -Constants::veh_Lr - Constants::bloating
                  && localXY.x() <= Constants::veh_Lf + Constants::bloating
                  && localXY.y() >= -Constants::veh_W / 2 - Constants::bloating
                  && localXY.y() <= Constants::veh_W / 2 + Constants::bloating;
    return inside;
}

bool Replanning::checkCollision( const Eigen::Vector3d &veh_obs, const traj &curr_pose ) {
    // 判断车辆四角点是否存在某点在障碍物矩形框内
    Eigen::Vector2d curr_state( curr_pose.x, curr_pose.y );
    Eigen::Vector2d point_0, point_1, point_2, point_3;
    Eigen::Matrix2d Rt;
    Rt << cos( curr_pose.phi ), -sin( curr_pose.phi ), sin( curr_pose.phi ), cos( curr_pose.phi );
    point_0 = curr_state + Rt * Eigen::Vector2d( -Constants::veh_Lr, Constants::veh_W / 2 );
    // RCLCPP_INFO(rclcpp::get_logger("replanning.cpp"), "point_0 [%.3f,%.3f]", point_0.x(), point_0.y());
    if ( isPointInRectangle( point_0, veh_obs ) )
        return true;  // 碰撞

    point_1 = curr_state + Rt * Eigen::Vector2d( Constants::veh_Lf, Constants::veh_W / 2 );
    // RCLCPP_INFO(rclcpp::get_logger("replanning.cpp"), "point_1 [%.3f,%.3f]", point_1.x(), point_1.y());
    if ( isPointInRectangle( point_1, veh_obs ) )
        return true;

    point_2 = curr_state + Rt * Eigen::Vector2d( Constants::veh_Lf, -Constants::veh_W / 2 );
    // RCLCPP_INFO(rclcpp::get_logger("replanning.cpp"), "point_2 [%.3f,%.3f]", point_2.x(), point_2.y());
    if ( isPointInRectangle( point_2, veh_obs ) )
        return true;

    point_3 = curr_state + Rt * Eigen::Vector2d( -Constants::veh_Lr, -Constants::veh_W / 2 );
    // RCLCPP_INFO(rclcpp::get_logger("replanning.cpp"), "point_3 [%.3f,%.3f]", point_3.x(), point_3.y());
    if ( isPointInRectangle( point_3, veh_obs ) )
        return true;

    return false;
}

bool Replanning::isActiveReplanning( const traj &current_pos_ ) {
    int    current_nearest_index = 0;
    double min_dist              = ( double )INT_MAX;
    for ( int i = 0; i < last_ref_traj.size(); i++ ) {
        double dist = sqrt( pow( ( current_pos_.x - last_ref_traj[ i ].x ), 2 )
                            + pow( ( current_pos_.y - last_ref_traj[ i ].y ), 2 ) );
        if ( dist < min_dist ) {
            min_dist              = dist;
            current_nearest_index = i;
        }
    }
    // 几何方式判断
    for ( const Eigen::Vector3d &veh_obs : vehicle_obstacles ) {
        for ( int i = current_nearest_index; i < last_ref_traj.size(); i++ ) {
            double dist = sqrt( pow( ( veh_obs.x() - last_ref_traj[ i ].x ), 2 )
                                + pow( ( veh_obs.y() - last_ref_traj[ i ].y ), 2 ) );
            if ( dist > 5.0 ) {
                continue;
            }
            if ( checkCollision( veh_obs, last_ref_traj[ i ] ) )
                return true;
        }
    }

    // 栅格地图方式判断

    return false;
}

std::vector< int > Replanning::findStartAndTargetState( const traj &current_pose ) {
    int start_index   = 0;
    start_state       = Eigen::Vector3d( current_pose.x, current_pose.y, current_pose.phi );
    double start_dist = ( double )INT_MAX;
    for ( int i = 0; i < last_ref_traj.size(); i++ ) {
        double dist = sqrt( pow( ( start_state.x() - last_ref_traj[ i ].x ), 2 )
                            + pow( ( start_state.y() - last_ref_traj[ i ].y ), 2 ) );
        if ( dist < start_dist ) {
            start_dist  = dist;
            start_index = i;
        }
    }

    // 选取原始参考路径上距离障碍物一定距离的某点作为重规划终点
    Eigen::Vector3d veh_obs      = vehicle_obstacles.back();
    double          min_d        = Constants::dist_from_obs2target;
    int             target_index = 0;
    for ( int i = last_ref_traj.size() - 1; i >= 0; i-- ) {
        double dist =
            sqrt( pow( ( veh_obs.x() - last_ref_traj[ i ].x ), 2 ) + pow( ( veh_obs.y() - last_ref_traj[ i ].y ), 2 ) );
        if ( dist < min_d ) {
            target_state = Eigen::Vector3d( last_ref_traj[ i ].x, last_ref_traj[ i ].y, last_ref_traj[ i ].phi );
            target_index = i;
            break;
        }
    }
    if ( last_ref_traj[ target_index ].vel < 0 ) {
        target_state.z() = Tools::normalize_angle(
            target_state.z() - M_PI );  // 倒车任务时从控制拿到的参考轨迹中航向角与实际航向角偏差了M_PI
    }
    // 选取原始参考路径最后一个点为重规划终点
    // traj ts     = last_ref_traj.back();
    // target_state = Eigen::Vector3d(ts.x, ts.y, ts.phi);
    RCLCPP_INFO( rclcpp::get_logger( "" ), "Replanning start and target is [%.3f,%.3f,%.3f] [%.3f,%.3f,%.3f]",
                 start_state.x(), start_state.y(), start_state.z(), target_state.x(), target_state.y(),
                 target_state.z() );

    if ( last_ref_traj[ start_index ].vel >= 0 )
        motion_type = 1;
    else
        motion_type = -1;

    return { start_index, target_index };
}

void Replanning::findStartAndTargetState( const Eigen::Vector3d &start_, const Eigen::Vector3d &goal_,
                                          int motion_type_ ) {
    start_state  = start_;
    target_state = goal_;
    motion_type  = motion_type_;
}

int Replanning::calculateNewStartState( int count, int histoty_start_index, double reverse_dist ) {
    double dist_between_two_point = 0.05;
    if ( histoty_start_index > 0 ) {
        traj point_0           = last_ref_traj[ histoty_start_index ];
        traj point_1           = last_ref_traj[ histoty_start_index - 1 ];
        dist_between_two_point = sqrt( pow( ( point_0.x - point_1.x ), 2 ) + pow( ( point_0.y - point_1.y ), 2 ) );
    }

    int new_start_index = histoty_start_index - ( int )( reverse_dist * ( count + 1 ) / dist_between_two_point );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "new_start_index: [%d]", new_start_index );
    if ( new_start_index < 0 )
        return 0;

    start_state = Eigen::Vector3d( last_ref_traj[ new_start_index ].x, last_ref_traj[ new_start_index ].y,
                                   last_ref_traj[ new_start_index ].phi );
    RCLCPP_INFO( rclcpp::get_logger( "" ), "new start is [%.3f,%.3f,%.3f]", start_state.x(), start_state.y(),
                 start_state.z() );

    return new_start_index;
}

bool Replanning::runReplanning() {
    RCLCPP_INFO( rclcpp::get_logger( "" ), "Create HybridAstar searcher" );
    HybridAStar searcher;

    RCLCPP_INFO( rclcpp::get_logger( "" ), "Initialize Parameters" );
    searcher.InitParam( start_state, target_state, motion_type, image_map, map_data );

    RCLCPP_INFO( rclcpp::get_logger( "" ), "Show map data" );
    searcher.showMapData();

    RCLCPP_INFO( rclcpp::get_logger( "" ), "Show obstacles info: size [%d]", vehicle_obstacles.size() );
    for ( const auto &veh_obs : vehicle_obstacles ) {
        RCLCPP_INFO( rclcpp::get_logger( "" ), "veh_obs: [%.3f, %.3f, %.3f]", veh_obs.x(), veh_obs.y(), veh_obs.z() );
    }

    RCLCPP_INFO( rclcpp::get_logger( "" ), "Begin search, motion type is [%d]", motion_type );
    auto time_start = std::chrono::high_resolution_clock::now();
    if ( !searcher.plan() )
        return false;
    auto                            time_end = std::chrono::high_resolution_clock::now();
    std::chrono::duration< double > diff     = ( time_end - time_start ) * 1000;
    RCLCPP_INFO( rclcpp::get_logger( "" ), "h_a_star cost time: %.5f ms", diff.count() );

    RCLCPP_INFO( rclcpp::get_logger( "" ), "Successfully get path!" );
    std::vector< Eigen::Vector4d > ha_path = searcher.returnPath();

    replanning_path.poses.clear();
    geometry_msgs::msg::PoseStamped pose;
    replanning_path.header.frame_id = "map";
    for ( int i = 0; i < ha_path.size(); i++ ) {
        pose.pose.position.set__x( ha_path[ i ].x() );
        pose.pose.position.set__y( ha_path[ i ].y() );
        tf2::Quaternion tf2_quat;
        tf2_quat.setRPY( 0, 0, ha_path[ i ].z() );
        geometry_msgs::msg::Quaternion goal_quat = tf2::toMsg( tf2_quat );
        pose.pose.orientation.x                  = goal_quat.x;
        pose.pose.orientation.y                  = goal_quat.y;
        pose.pose.orientation.z                  = goal_quat.z;
        pose.pose.orientation.w                  = goal_quat.w;
        replanning_path.poses.push_back( pose );
    }
    return true;
}

void Replanning::getReplanningPath( nav_msgs::msg::Path &path ) {
    path = replanning_path;
}
